#include "slerp.h"
#include <stdlib.h>
#include <math.h>

/******************************************************************************************************
*
*	METODO: slerp --> Realiza la interpolacion de dos cuaternios
*
*	ENTRADAS: float* quaternionA --> Representacion en el espacio (a, b, c, e) del cuaternio A
*		  float* quaternionB --> Representacion en el espacio (x, y, w, z) del cuaternio B
*		  double t	     --> Angulo de giro
*
*	SALIDAS:  float[] --> Represetacion en el espacio del cuaternio interpolado entre A y B
*
*	CREADOR:  Lhoriant
*	FECHA:	  24/10/2011
*******************************************************************************************************/

float* slerp(float* cuaternioA, float* cuaternioB, double t) {
	// Establecemos las matrices que representaran lso cuaternios
	float q1[4];
	float q2[4];
	static float cuaternioC[4];
	
	// Guardamos los datos de cada cuaternio en una matriz para que sea mas comodo su tratamiento
	for(int i=0; i<4; i++){
		q1[i] = cuaternioA[i];
		q2[i] = cuaternioB[i];
	}
	
	// Calculamos el angulo entre ambos cuaternios
	double angle = (q1[0]*q2[0]) + (q1[1]*q2[1]) + (q1[2]*q2[2]) + (q1[3]*q2[3]);
	
	//Si los cuaternios son iguales u opuestos, el angulo que forman es 0 y devolvemos el primer cuaternio
	if (abs(angle) >= 1.0){
		for (int i=0;i<4;i++){
            cuaternioC[i] = q1[i];
        }        
	}else{	
		//Calculamos los datos temporales que necesitamos para obtener el cuaternio resultante
		double halfTheta = acos(angle);
		double sinHalfTheta = sqrt(1.0 - angle*angle);
		// Comprobamos que theta sea distinto de 180 grados, si theta es 180 grados el resultado no esta definido y 
		//el giro se puede hacer sobre cualquier eje normal a cualquiera de los dos cuaternios
		if (fabs(sinHalfTheta) < 0.001){ // fabs --> Sirve para obetener el valor absoluto de un numero en coma flotante
			cuaternioC[0] = q1[0] * 0.5 + q2[0] * 0.5;
			cuaternioC[1] = q1[1] * 0.5 + q2[1] * 0.5;
			cuaternioC[2] = q1[2] * 0.5 + q2[2] * 0.5;
			cuaternioC[3] = q1[3] * 0.5 + q2[3] * 0.5;
		}else{
			double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
			double ratioB = sin(t * halfTheta) / sinHalfTheta; 
			//Calculamos el cuaternio
			cuaternioC[0] = q1[0] * ratioA + q2[0] * ratioB;
			cuaternioC[1] = q1[1] * ratioA + q2[1] * ratioB;
			cuaternioC[2] = q1[2] * ratioA + q2[2] * ratioB;
			cuaternioC[3] = q1[3] * ratioA + q2[3] * ratioB;
		}
	}	
	
	return cuaternioC;
}
